package de.fh.ka.as.car.routing;

import java.util.ArrayList;

import android.graphics.PointF;
import android.util.Log;

public class AutoPilot {

	private PointF lastPoint;

	public ArrayList<RouteCommand> calculateRoute(ArrayList<PointF> routePoints, double startPositionAngle) {
		double startAngle = 0;
		ArrayList<RouteCommand> commands = new ArrayList<RouteCommand>();
		for (int pointIndex = 0; pointIndex < routePoints.size(); pointIndex++) {

			if (pointIndex + 1 < routePoints.size()) {

				if (pointIndex == 0) {
					startAngle = startPositionAngle;
				}

				PointF start = routePoints.get(pointIndex);
				PointF target = routePoints.get(pointIndex + 1);

				// PointF vector1=new PointF(start.x-lastPoint.x,start.y-lastPoint.y);
				// PointF vector2=new PointF(target.x-start.x,target.y-start.y);

				double distance = calculateDistanceBetweenTwoPoints(start, target);
				double angle = calcAngleBetweenTwoPointsAndOrientationAngle(startAngle, start, target);
				startAngle += angle;
				// RouteCommand angleCommand;
				// if (angle < 0) { // turn left
				// angleCommand = new TurnCommand(start, target, angle * -1);
				// commands.add(angleCommand);
				// } else if (angle > 0) { // turn right
				// angleCommand = new TurnCommand(start, target, angle);
				// commands.add(angleCommand);
				// }
				RouteCommand angleCommand = new TurnCommand(start, target, angle);
				commands.add(angleCommand);

				RouteCommand driveCommand = new DriveForwardCommand(start, target, distance);
				commands.add(driveCommand);

				lastPoint = start;
			}
		}

		return commands;
	}

	// public ArrayList<RouteCommand> calculateRoute(ArrayList<PointF> routePoints,double startPositionAngle) {
	//
	// ArrayList<RouteCommand> commands = new ArrayList<RouteCommand>();
	// for (int pointIndex = 0; pointIndex < routePoints.size(); pointIndex++) {
	//
	//
	// if (pointIndex + 1 < routePoints.size()) {
	// PointF start = routePoints.get(pointIndex);
	// PointF target = routePoints.get(pointIndex + 1);
	//
	// PointF vector1=new PointF(start.x-lastPoint.x,start.y-lastPoint.y);
	// PointF vector2=new PointF(target.x-start.x,target.y-start.y);
	//
	// if(pointIndex==0){
	// vector1= calculateEinheitsVector(startPositionAngle);
	// }
	//
	// double angle=calculateAngleBetweenTwoVectors2(vector1,vector2);
	// double distance= calculateDistanceBetweenTwoPoints(start, target);
	//
	// RouteCommand angleCommand;
	// if(angle<0){
	// angleCommand= new RouteCommand(CommandType.TurnLeftAngle,angle);
	// }else{
	// angleCommand= new RouteCommand(CommandType.TurnRightAngle,angle);
	// }
	// commands.add(angleCommand);
	//
	// RouteCommand newCommand = new RouteCommand(CommandType.DriveDistance,distance);
	//
	// commands.add(newCommand);
	//
	// lastPoint=start;
	// }
	// }
	//
	// return commands;
	// }

	public static double calculateAngleBetweenTwoVectors(PointF start, PointF target) {
		double angle = calculateInverseCosinus((start.x * target.x + start.y * target.y)
				/ ((Math.sqrt(start.x * start.x + start.y * start.y) * Math.sqrt(target.x * target.x + target.y
						* target.y))));
		return angle;
	}

	public static double calculateAngleBetweenTwoVectors2(PointF start, PointF target) {
		double angle = calculateInverseCosinus((start.x * target.x + start.y * target.y)
				/ ((Math.sqrt(start.x * start.x + start.y * start.y) * Math.sqrt(target.x * target.x + target.y
						* target.y))));

		// calculate cross product to get angle direction
		double crossProduct = start.x * target.y - start.y * target.x;
		if (crossProduct < 0) {
			angle = angle * -1;
		}
		return angle;
	}

	public static double calculateDistanceBetweenTwoPoints(PointF start, PointF target) {
		return Math.sqrt(Math.pow(target.x - start.x, 2) + Math.pow(target.y - start.y, 2));
	}

	private static double calculateInverseCosinus(double value) {
		Log.i("Test", " " + value + " " + Math.acos(value));
		// if (value >= 0)
		// return Math.acos(value)* 180/Math.PI;
		// else
		// return (Math.acos(value)* 180/Math.PI) * -1;
		return Math.acos(value) * 180 / Math.PI;
	}

	public static PointF calculateEinheitsVector(double angle) {
		double test = Math.sin(0);
		// test=Math.sin(Math.PI/2);
		// test=Math.sin(Math.PI/2);
		// test=Math.sin((angle/180.0)*Math.PI);

		double xValue = Math.sin((angle / 180.0) * Math.PI);
		double yValue = -Math.cos((angle / 180.0) * Math.PI);
		PointF result = new PointF();
		result.x = (float) xValue;
		result.y = (float) yValue;
		return result;
	}

	private static double calcAngleBetweenTwoPointsAndOrientationAngle(double startAngle, PointF point1,
			PointF point2) {
		PointF vector1 = AutoPilot.calculateEinheitsVector(startAngle);

		PointF vector2 = new PointF(point2.x - point1.x, point2.y - point1.y);
		double angle = AutoPilot.calculateAngleBetweenTwoVectors2(vector1, vector2);
		Log.i("angle for startangle " + startAngle + " : ", "" + angle);

		return angle;
	}

}
